#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import serial
import time
import scipy
import numpy as np
from DelphinROSv2.msg import tsl_setpoints
from std_msgs.msg import Float32
from std_msgs.msg import Bool
from DelphinROSv2.msg import compass as compass_out
from scipy import stats

#### DEFINE GLOBAL VARIABLES ####
global flag
global cur_depthd
global cur_compass
global controller_onOff

########## LOW LEVEL CONTROL ############################################################
def PID_loop():

    #### SETUP ####
	global flag
	global cur_depthd
	global cur_compass
	global controller_onOff
        
        flag = False
        cur_depthd = Float32()
        cur_compass = compass_out()
        setpoint = 0
	controller_onOff=Bool()

        int_gain = rospy.get_param(rospy.resolve_name('integral-gain'))
        deriv_gain = rospy.get_param(rospy.resolve_name('deriv-gain'))
        prop_gain = rospy.get_param(rospy.resolve_name('prop-gain'))
        max_sat = rospy.get_param(rospy.resolve_name('max-saturation'))
        min_sat = rospy.get_param(rospy.resolve_name('min-saturation'))
        max_int_sat = rospy.get_param(rospy.resolve_name('max-int-saturation'))
        min_int_sat = rospy.get_param(rospy.resolve_name('min-int-saturation'))
        max_prop_sat = rospy.get_param(rospy.resolve_name('max-prop-saturation'))
        min_prop_sat = rospy.get_param(rospy.resolve_name('min-prop-saturation'))

	min_int_error = min_int_sat / int_gain
	max_int_error = max_int_sat / int_gain
	int_error = 0

        pitch_int_error = 0
	pitch_int_gain = 0.0

	time_zero = time.time()	

	der_array_size = 100
	time_array = np.zeros(der_array_size)
	sample = np.zeros(der_array_size)


        #### MAIN CONTROLLER LOOP ####
        while not rospy.is_shutdown():
		if flag == True:
                        #print ' flag is now true! running main loop...........'
	
			#### BASIC CALCS ####
			dt = time.time() - time_zero				# Calculate dt time between last loop interation
			time_zero = time.time()					# Record time of start of this loop
			error = (cur_depthd.data - cur_compass.depth)		# Calculate error between demand and actual

			#### PROPORTIONAL TERM ####
			pTerm = error * prop_gain				# Calculate proportional term

			if pTerm < min_prop_sat:				# Limit proportional term
			   pTerm = min_prop_sat
			elif pTerm > max_prop_sat:
			   pTerm = max_prop_sat
			#print 'prop term %f' %(pTerm)

			#### INTEGRAL ####
			int_error += dt*error					# Calculate the integral error

			if int_error < min_int_error:				# Limit integral error. Nb - Not the integral term
			   int_error = min_int_error	
			elif int_error > max_int_error:
			   int_error = max_int_error

			iTerm = int_error*int_gain				# Calculate integral term
			
			#### DERIVATIVE ####
			time_array[1:der_array_size] = time_array[0:(der_array_size-1)]		# Shift old values up in the array
			time_array[0] = dt					# Set first array term to new dt value
	
			sample[1:der_array_size] = sample[0:(der_array_size-1)]	# Shift old values up in the array
			sample[0] = cur_compass.depth				# Set first array term to new depth value

			cumulative_time = np.zeros(der_array_size)		# Reset cumulative time array to zero
	
			i = 1
                        #print 'entering extra while loop.....'
			while i < der_array_size:				# Create cumulative time array
				cumulative_time[i] = cumulative_time[i-1] + time_array[i]
				i += 1
                        #print 'exiting while loop...........'

			derivative, intercept, r_value, p_value, std_err = stats.linregress(cumulative_time,sample)	# Calculate linear least squares fit			
			
			#print 'after regression'
			
			dTerm = derivative * deriv_gain				# Calculate dTerm
                        #print 'derivative', derivative
                        #print 'r_value', r_value

			#### SUM TERMS ####
                        if not np.isnan(dTerm):
                            #print 'dTerm exists: ', dTerm
                            total = pTerm + iTerm + dTerm			# Calculate total linear controller output
                        else:
                            #print 'dTerm not exist'
                            total = pTerm + iTerm
                            
                        #print 'after isnan......'

			
			#### PITCH CONTROLLER ####
                        pitch_error = (0 - cur_compass.pitch)
			pitch_p = pitch_error*0.0001
			#print 'pitch p: ',pitch_p                        

			pitch_int_error += pitch_error * 0.0000001											# Calculate the integral error
			pitch_int_gain = 0.05
			pitch_int_sat = 4
			pitch_int_error_sat = pitch_int_sat/pitch_int_gain
			
			if pitch_int_error < -pitch_int_error_sat:		# Limit integral error. Nb - Not the integral term
			   pitch_int_error = -pitch_int_error_sat	
			elif pitch_int_error > pitch_int_error_sat:
			   pitch_int_error = pitch_int_error_sat

			pitch_i = pitch_int_error*pitch_int_gain
			#print 'Pitch i: ',pitch_i
                        
                        pitch_total = pitch_p + pitch_i
			#print 'pitch total: ',pitch_total                        
                        
					
                            
			#### FORCE TO THRUSTER SP:EED EQN ####
                       # print 'Total', total
                        
			setpoint0 = np.sign(total)*(60*(np.abs(0.91*total+pitch_total)/(1000*0.46*0.07**4))**0.5)
			setpoint1 = np.sign(total)*(60*(np.abs(1.11*total)/(1000*0.46*0.07**4))**0.5)

			#### PRINT TERMS ####
			#print 'Proportional term:', pTerm
			#print 'Integral term: ', iTerm
			#print 'Derivative term: ', dTerm
			#print 'Setpoint: ', setpoint

			#### PUBLISH SETPOINTS ####
	                output = tsl_setpoints()
		        output.thruster0 = int(setpoint0)
		        output.thruster1 = int(setpoint1)
                        output.pterm = pTerm
                        output.iterm = iTerm
                        output.dterm = dTerm
                        #output.total = total
			if controller_onOff==True:
			        pub.publish(output)
                        
                        
			flag = False
                        #print 'flag should be false now...', flag


    #### END OF MAIN CONTROLLER LOOP ####

######## END OF LOW LEVEL CONTROLLER #####################################################


#### WHEN NEW MISSION OR COMPASS DATA ARRIVES UPDATE VALUES AND RESET FLAG ####
def depth_demand_cb(depthd):
    global flag
    global cur_depthd
    cur_depthd = depthd
    flag = True   

def compass_cb(compass):
    global flag
    global cur_compass
    cur_compass = compass   
    #print 'new compass data!!' 
    flag = True

def onOff_cb(onOff):
    global controller_onOff
    global flag
    controller_onOff=onOff.data
    flag=True
    #print 'new controller onOff'

#### INITIALISATION ####
if __name__ == '__main__':
    rospy.init_node('PID_depth_control')
   
    rospy.Subscriber('depth_demand', Float32, depth_demand_cb)
    rospy.Subscriber('/compass_out', compass_out, compass_cb)
    rospy.Subscriber('Depth_onOFF', Bool, onOff_cb)
    pub = rospy.Publisher('TSL_setpoints_vertical', tsl_setpoints)
    rospy.loginfo("Depth controller (hover) online")

    PID_loop()
